#include "Arm.h"

Arm::Arm(int clientID, int baseJoint, int shoulderJoint, int elbowJoint)
{
	this->k = Kinematics(10, 2, 3, 5);
	this->clientID = clientID;
	this->baseJoint = baseJoint;
	this->shoulderJoint = shoulderJoint;
	this->elbowJoint = elbowJoint;
	this->teta1 = 0;
	this->teta2 = 0;
	this->teta3 = 0;
	this->error = 0;
	this->delta = 10.f / 180 * M_PI;
	this->speed = 100;
	this->force = 100;
}


Arm::~Arm(void)
{
}

void Arm::goToRestPosition()
{
	printf("FUNCTION: goToRestPosition\n");

	setSpeedForce(this->speed, this->force);

	error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_streaming );
	error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_streaming );
	error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_streaming );
	printf("Sono in teta1: %f\n", teta1*180/M_PI);
	printf("Sono in teta2: %f\n", teta2*180/M_PI);
	printf("Sono in teta3: %f\n", teta3*180/M_PI);
	
	error = simxSetJointTargetVelocity(clientID, baseJoint, speed, simx_opmode_oneshot);
	error = simxSetJointTargetVelocity(clientID, shoulderJoint, speed, simx_opmode_oneshot);
	error = simxSetJointTargetVelocity(clientID, elbowJoint, speed, simx_opmode_oneshot);
		
	error = simxSetJointTargetPosition(clientID, baseJoint, 0, simx_opmode_oneshot);
	error = simxSetJointTargetPosition(clientID, shoulderJoint, 0, simx_opmode_oneshot);
	error = simxSetJointTargetPosition(clientID, elbowJoint, 0, simx_opmode_oneshot);
	printf("Vado in posizione start\n");

	while(!(teta1 <= delta && teta1 >= -delta && teta2 <= delta && teta2 >= -delta && teta3 <= delta && teta3 >= -delta))
	{
		Sleep(2000);
		error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_streaming );
		error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_streaming );
		error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_streaming );
	}
	printf("Sono in posizione start\n\n");
}

void Arm::goToPoint(Point p)
{
	printf("FUNCTION: goToPoint %f, %f, %f\n", p.getValue(Point::X), p.getValue(Point::Y), p.getValue(Point::Z));

	//setSpeedForce(this->speed, this->force);

	error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_buffer);
	printf("Sono in teta1: %f\n", teta1*180/M_PI);
	printf("Sono in teta2: %f\n", teta2*180/M_PI);
	printf("Sono in teta3: %f\n", teta3*180/M_PI);

	std::vector<float> joints = k.inverse(p);		

	float angle_base = joints.at(0);
	float angle_shoulder = joints.at(1);
	float angle_elbow = joints.at(2);

	if(!((teta1<=angle_base+delta)&&(teta1>=angle_base-delta)&&(teta2<=angle_shoulder+delta)&&(teta2>=angle_shoulder-delta)&&(teta3<=angle_elbow+delta)&&(teta3>=angle_elbow-delta)))
	{
		error = simxSetJointTargetVelocity(clientID, baseJoint, speed, simx_opmode_oneshot);
		error = simxSetJointTargetVelocity(clientID, shoulderJoint, speed, simx_opmode_oneshot);
		error = simxSetJointTargetVelocity(clientID, elbowJoint, speed, simx_opmode_oneshot);

		error = simxSetJointTargetPosition(clientID, baseJoint, angle_base, simx_opmode_oneshot);
		error = simxSetJointTargetPosition(clientID, shoulderJoint, angle_shoulder, simx_opmode_oneshot);
		error = simxSetJointTargetPosition(clientID, elbowJoint, angle_elbow, simx_opmode_oneshot);
		printf("Vado in teta1: %f\n", angle_base*180/M_PI);
		printf("Vado in teta2: %f\n", angle_shoulder*180/M_PI);
		printf("Vado in teta3: %f\n", angle_elbow*180/M_PI);
	}
}

void Arm::followPath(std::vector<Point> p) 
{
		Emg emgCtr = Emg();
		printf("FUNCTION: followPath\n");

		setSpeedForce(this->speed, this->force);

		for(int i = 0; i <= p.size()-1; i++)
		{
			if(!emgCtr.getStop()){
			//	printf("Punto %d: \n", i);
				this->goToPoint(p[i]);
				//Sleep(2000);
				std::vector<float> joints = k.inverse(p[i]);
				float angle_base = joints[0];
				float angle_shoulder = joints[1];
				float angle_elbow = joints[2];

				while(!((teta1<=angle_base+delta)&&(teta1>=angle_base-delta)&&(teta2<=angle_shoulder+delta)&&(teta2>=angle_shoulder-delta)&&(teta3<=angle_elbow+delta)&&(teta3>=angle_elbow-delta)))
				{
					//printf("delta: %f\n", delta);
					//printf("teta1: %d, teta2 %d, teta3 %d \n", (teta1<=angle_base+delta)&&(teta1>=angle_base-delta),(teta2<=angle_shoulder+delta)&&(teta2>=angle_shoulder-delta),(teta3<=angle_elbow+delta)&&(teta3>=angle_elbow-delta));
					Sleep(2000);
					error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_buffer);
					error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_buffer);
					error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_buffer);
					printf("Sono in teta1: %f e devo andare in %f\n", teta1*180/M_PI, angle_base*180/M_PI);
					printf("Sono in teta2: %f e devo andare in %f\n", teta2*180/M_PI, angle_shoulder*180/M_PI);
					printf("Sono in teta3: %f e devo andare in %f\n", teta3*180/M_PI, angle_elbow*180/M_PI);
				}
			}
			else {
				setSpeedForce(0, force);
				return;
			}	
		}
		printf("\n");
		return;
}

void Arm::move(int J, float degree)
{
	printf("FUNCTION: move\n");

	setSpeedForce(this->speed, this->force);

	int joint = 0;
	float teta = 0;

	error = simxGetJointPosition(clientID, baseJoint, &teta1, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, shoulderJoint, &teta2, simx_opmode_buffer);
	error = simxGetJointPosition(clientID, elbowJoint, &teta3, simx_opmode_buffer);

	printf("Sono in teta1: %f\n", teta1*180/M_PI);
	printf("Sono in teta2: %f\n", teta2*180/M_PI);
	printf("Sono in teta3: %f\n", teta3*180/M_PI);
		
	switch(J){
		case 0:
			joint = baseJoint;
			teta = teta1;
			break;
		case 1:
			joint = shoulderJoint;
			teta = teta2;
			break;
		case 2:
			joint = elbowJoint;
			teta = teta3;
			break;
		default:
			return;
	}
	printf("Muovo joint %d di %f rad\n\n", (joint+1), teta);
	error = simxSetJointTargetPosition(clientID, joint, teta+(degree/180.f*M_PI), simx_opmode_oneshot);
}

void Arm::setSpeedForce(float speed, float force){
	error = simxSetJointTargetVelocity(clientID, baseJoint, speed, simx_opmode_oneshot);
	error = simxSetJointTargetVelocity(clientID, shoulderJoint, speed, simx_opmode_oneshot);
	error = simxSetJointTargetVelocity(clientID, elbowJoint, speed, simx_opmode_oneshot);

	error = simxSetJointForce(clientID, baseJoint, force, simx_opmode_oneshot);
	error = simxSetJointForce(clientID, shoulderJoint, force, simx_opmode_oneshot);
	error = simxSetJointForce(clientID, elbowJoint, force, simx_opmode_oneshot);
}





